#include "user_lib.h"
#include "usart.h"
#include "pid.h"
#include "bsp_can.h"

/**
  * @brief          一阶低通滤波初始化
  * @author         yun
  * @param[in]      一阶低通滤波结构体
  * @param[in]      间隔的时间，单位 s
  * @param[in]      滤波参数
  * @retval         返回空
  */
void first_order_filter_init(first_order_filter_type_t *first_order_filter_type, float frame_period, const float num)
{
    first_order_filter_type->frame_period = frame_period;
    first_order_filter_type->num = num;
    first_order_filter_type->input = 0.0f;
    first_order_filter_type->out = 0.0f;
}

/**
  * @brief          一阶低通滤波计算
  * @author         yun
  * @param[in]      一阶低通滤波结构体
  * @param[in]      间隔的时间，单位 s
  * @retval         返回空
  */
void first_order_filter_cali(first_order_filter_type_t *first_order_filter_type, float input)
{
	first_order_filter_type->last_out = first_order_filter_type->out;
    first_order_filter_type->input = input;
    first_order_filter_type->out =
    first_order_filter_type->num / ( first_order_filter_type->num + 	first_order_filter_type->frame_period )
	* first_order_filter_type->out + 
	first_order_filter_type->frame_period / (first_order_filter_type->num + 
	first_order_filter_type->frame_period) * first_order_filter_type->input;
//					first_order_filter_type->input * first_order_filter_type->num[0]+(1-first_order_filter_type->num[0])*first_order_filter_type->last_out;
}



/**
  * @brief          按键重复调用
  * @param          键盘按键
  * @param          客户端按键反馈数据       
  */
static int32_t cnt = 0;
static int16_t i   = 0;
int key_count(uint16_t key, uint16_t key_target)
{
	if(key == 0)
	{
		i = 1;
	}
	if(key == key_target && i == 1)
	{
		cnt++;
		i = 0;
	}
	
	if(cnt % 2 == 1)
		return 1;
	else
		return 0;
}


/*******************************************************************************************	
  * @Brief  			延时函数
  * @Param				延时时间
  * @Retval
  * @Date
 *******************************************************************************************/
void my_delay(int delay_time)
{
	while(i--)
	{
		;
	}
}




/**
  * @brief    			斜坡函数初始化
  * @param    			斜坡函数结构体
  * @param    			间隔时间
  * @param    			斜坡目标值
  * @param    			斜坡源
  */
void ramp_init(ramp_function_source_t *ramp_source_type, float frame_period, float max, float min)
{
    ramp_source_type->frame_period = frame_period;
    ramp_source_type->max_value = max;
    ramp_source_type->min_value = min;
    ramp_source_type->input = 0.0f;
    ramp_source_type->out = 0.0f;
}

void ramp_calc(ramp_function_source_t *chassis_ramp, float input)
{
    chassis_ramp->input = input;
	if(input != 0)
	{
    chassis_ramp->out += chassis_ramp->input * chassis_ramp->frame_period;
    if (chassis_ramp->out > chassis_ramp->max_value)
        chassis_ramp->out = chassis_ramp->max_value;
    else if (chassis_ramp->out < chassis_ramp->min_value)
        chassis_ramp->out = chassis_ramp->min_value;
	}
	else
	{
		if(chassis_ramp->out > 5)
		  chassis_ramp->out -= 10.0f;
		else if(chassis_ramp->out <-5)
			chassis_ramp->out += 10.0f;
		else
			chassis_ramp->out = 0;
	}
}





